/*
 * libmekaskin : a library for Meka Robotics' skin sensor.
 * Copyright (C) 2012  Meka Robotics LLC
 * 
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */
#include "TaxelArray.h"

#include <iostream>

#include <fcntl.h>
#include <errno.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <sys/ioctl.h>

TaxelArray::TaxelArray(std::string portName) throw(TaxelIOException)
: portState(ProtocolFsmStates::STARTING)
{
    fd = open(portName.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd < 0) {
        throw TaxelIOException(strerror(errno));
    }

    setOptions();

    FD_ZERO(&input);
    FD_SET(fd, &input);
    nfds = fd + 1;
}

void TaxelArray::setOptions() throw(TaxelIOException)
{
    // Flush the input and output queues
    tcflush(fd, TCIOFLUSH);

    struct termios options;

    // Get the current options for the port
    tcgetattr(fd, &options);
 
    // Set the baud rates to 57600
    cfsetispeed(&options, B57600);
    cfsetospeed(&options, B57600);
 
    // Enable the receiver and set local mode
    options.c_cflag |= (CLOCAL | CREAD);
    options.c_cflag &= ~PARENB;
    options.c_cflag &= ~CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;

    // Set the new options for the port      
    if (tcsetattr(fd, TCSANOW, &options) < 0) {
        throw TaxelIOException(strerror(errno));
    }
}

TaxelArray::~TaxelArray() {
    close(fd);
}

void TaxelArray::registerTaxelObserver(TaxelArrayObserverPtr observer)
{
    observers.push_back(observer);
}

size_t TaxelArray::getNumberBytesAvailable()
{
    int nbytesAvail;
    if (ioctl(fd, FIONREAD, &nbytesAvail) < 0) {
        nbytesAvail = 0;
    }

    if ((size_t) nbytesAvail > INCOMING_BUFFER_SIZE) {
        nbytesAvail = INCOMING_BUFFER_SIZE;
    }

    return nbytesAvail;
}

uint16_t TaxelArray::uint8ToUint16(uint8_t v1, uint8_t v2)
{
    union { 
        uint8_t bytes[2];
        uint16_t word;
    } conv;

    conv.bytes[0] = v1;
    conv.bytes[1] = v2;

    return conv.word;
}

void TaxelArray::processIncomingBytes(uint8_t* data, size_t nbytes)
{
    for (int i = 0; i < (int) nbytes; i++) {
        if ((portState == ProtocolFsmStates::STARTING) && (data[i] == 's')) {
            portState = ProtocolFsmStates::RECEIVING;
            continue;
        }

        if (portState == ProtocolFsmStates::RECEIVING) {
            partialMessage.push_back(data[i]);

            if (partialMessage.size() == SKIN_PATCH_PAYLOAD_SIZE) {
                portState = ProtocolFsmStates::FINISHING;
                continue;
            }
        }

        if (portState == ProtocolFsmStates::FINISHING) {
            // Check stop byte 
            if (data[i] != 'f') {
                //std::cerr << "Bad stop character : \"" << (int) data[i] << "\"" << std::endl;
                message.clear();
                partialMessage.clear();
                portState = ProtocolFsmStates::STARTING;  
                return;
            }

            int localChkSum = 0;
            for (int j = 0; j < SKIN_PATCH_NUMBER_SENSORS; j++) {
                uint16_t value = uint8ToUint16(partialMessage[j*2], partialMessage[j*2+1]);
                message.push_back(value);
                //std::cerr << (int) value << " ";

                localChkSum += value;
                if (localChkSum > 65535) {
                    localChkSum -= 65536;
                }
            }
            //std::cerr << std::endl;
            
            uint16_t deviceChkSum = uint8ToUint16(partialMessage[SKIN_PATCH_NUMBER_SENSORS*2], partialMessage[SKIN_PATCH_NUMBER_SENSORS*2+1]);
            if ((uint16_t) localChkSum == deviceChkSum) {
                // Notify 
                notifyReadingReceived();
            }

            // Reset state machine
            message.clear();
            partialMessage.clear();
            portState = ProtocolFsmStates::STARTING;  
        }

    }
}

void* TaxelArray::processEventLoop(void* data)
{
    TaxelArray* self = (TaxelArray*) data;

    while(!self->stopped) {
        int ret = select(self->nfds, &self->input, NULL, NULL, NULL);
        if (ret < 0) {
            // FIXME handle this case differently
            continue;
        }

        if (FD_ISSET(self->fd, &self->input)) {
                // Find the number of bytes available in the input buffer
                unsigned nbytesAvail = self->getNumberBytesAvailable();
                
                // Read that many bytes all in one chunk
                uint8_t buffer[INCOMING_BUFFER_SIZE];
                int nbytes = read(self->fd, buffer, nbytesAvail);
                if (nbytes < 0) {
                    // TODO handle this case differently
                    continue;
                }

                self->processIncomingBytes(buffer, nbytes);                
        } 
    }

   std::cout << "Thread stopped" << std::endl;
}

void TaxelArray::start()
{
    stopped = false;
    pthread_create(&eventThread, NULL, &TaxelArray::processEventLoop, this);      
}

void TaxelArray::stop()
{
    stopped = true;
    pthread_join(eventThread, NULL);
}

void TaxelArray::notifyReadingReceived()
{
      auto it = observers.begin();
      while(it != observers.end())
      {
         if ((*it).expired()) {
            it = observers.erase(it);
         } else {
            std::shared_ptr<TaxelArrayObserver> obs = (*it).lock(); // create a shared_ptr from the weak_ptr
            obs->ReadingReceived(message);
            ++it;
         }
      }
}

unsigned int TaxelArray::getNumberTaxel()
{
    // TODO For the current hardware version, this
    // value is fixed. Needs to be made dynamic/introspectable for
    // future versions.
    return 12;
}

unsigned int TaxelArray::getNumberPatch()
{
    // TODO Have the hardware report this 
    return 1;
}

std::string TaxelArray::getProductVersion()
{
    // TODO Get real pid
    return "1.0";    
}

std::string TaxelArray::getProductName()
{
    // TODO Get real pid
    return "Skin proto"; 
}


